
#include <comm/canopen.h>
#include <socketcan_interface/threading.h>
#include <socketcan_interface/string.h>

// initializing instancePtr with NULL
Canopen *Canopen ::instancePtr = NULL;

Canopen::Canopen(std::shared_ptr<rclcpp::Node> node):
    node_(node)
{
    memset(&m_commPort, 0, sizeof(COMM_PORT));
}

Canopen::~Canopen()
{
    m_quit_mutex.unlock();
    m_thread.join();
    m_sdoSend_quit_mutex.unlock();
    sdoSendThread.join();
}

int Canopen::nodeIdToIdx(uint8_t nodeId)
{
    for(uint8_t i=0; i<CANOPEN_NODES; i++)
    {
        if(nodeId == m_commPort.cfg[i].nodeId)
        {
            return i;
        }
    }
    return -1;
}

uint16_t Canopen::getStatusWord(uint8_t nodeId)
{
    int idx = nodeIdToIdx(nodeId);
    if( idx < 0)
    {
        return 0;
    }
    return m_commPort.tPdoList[idx].pdo604100;
}

int Canopen::getEnc(uint8_t nodeId)
{
    int idx = nodeIdToIdx(nodeId);
    if( idx < 0)
    {
        return -1;
    }
    return m_commPort.tPdoList[idx].pdo606300;
}

//test 
int16_t Canopen::getAngle(uint8_t nodeId)
{
    int idx = nodeIdToIdx(nodeId);
    if( idx < 0)
    {
        return -1;
    }
    return m_commPort.tPdoList[idx].pdo310200;
}
uint32_t Canopen::getOdomter(uint8_t nodeId)
{
    int idx = nodeIdToIdx(nodeId);
    if( idx < 0)
    {
        return -1;
    }
    return m_commPort.tPdoList[idx].pdo310100;
}

uint32_t Canopen::getPvel(uint8_t nodeId)
{
    int idx = nodeIdToIdx(nodeId);
    if( idx < 0)
    {
        return 1;
    }
    return m_commPort.tPdoList[idx].pdo608100;
}

int Canopen::getVvel(uint8_t nodeId, int type)
{
    int idx = nodeIdToIdx(nodeId);
    if( idx < 0)
    {
        return -1;
    }
    if(type == 0)
    {
        return m_commPort.tPdoList[idx].pdo606c00;
    }
    else if (type == 1)
    {
        return m_commPort.tPdoList[idx].pdo210100;
    }
    else if (type == 2)
    {
        return m_commPort.tPdoList[idx].pdo310000;
    }
    return 0;
}

uint16_t Canopen::getErrCode(uint8_t nodeId)
{
    int idx = nodeIdToIdx(nodeId);
    if( idx < 0)
    {
        return -1;
    }
    return m_commPort.tPdoList[idx].pdo603f00;   
}

uint32_t Canopen::getDiStatus(uint8_t nodeId, int type)
{
    int idx = nodeIdToIdx(nodeId);
    if( idx < 0)
    {
        return -1;
    }
    if(type == 0)
    {
        return m_commPort.tPdoList[idx].pdo60FD00; //4byte
    }
    else if (type == 1)
    {
        return m_commPort.tPdoList[idx].pdo210200; //2yte
    }
    return 0;
}

int8_t Canopen::getActualCtlMode(uint8_t nodeId)
{
    int idx = nodeIdToIdx(nodeId);
    if( idx < 0)
    {
        return -1;
    }
    return m_commPort.tPdoList[idx].pdo606100;      
}

void Canopen::comm_PdoStart(bool startFlag)
{
    m_commPort.ctl.pdoStartFlag = startFlag;
}

void Canopen::nodeGuarding_send(uint8_t nodeId)
{
    can::Frame can_frame;
    can_frame.id = 0x700 + nodeId;
    can_frame.is_extended = false;
    can_frame.is_rtr = true; //must be remote frame
    can_frame.is_error = false;
    can_frame.dlc = 0;
    driver_->send(can_frame);     
}

void Canopen::nmtSend(uint8_t node, uint8_t cmd)
{
    can::Frame can_frame;
    can_frame.id = 0x00;
    can_frame.is_extended = false;
    can_frame.is_rtr = false;
    can_frame.is_error = false;
    can_frame.dlc = 2;
    can_frame.data[0] = cmd;
    can_frame.data[1] = node;
    driver_->send(can_frame);
    RCLCPP_INFO(node_->get_logger(), "nmt operation send cmd:%d to node %d",cmd, node);  
}

void Canopen::syncSend()
{
    can::Frame can_frame;
    can_frame.id = 0x80;
    can_frame.is_extended = false;
    can_frame.is_rtr = false;
    can_frame.is_error = false;
    can_frame.dlc = 0;
    driver_->send(can_frame); 
    // RCLCPP_INFO(node_->get_logger(), "sync send"); 
}

void Canopen::sdoSend(uint8_t node, uint8_t rwflag, uint8_t len, uint16_t index, uint8_t subIndex, uint32_t data)
{
    can::Frame can_frame;
    can_frame.id = (0x600 + node);
    can_frame.is_extended = false;
    can_frame.is_rtr = false;
    can_frame.is_error = false;
    can_frame.dlc = 8;
    for(int i=0; i<can_frame.dlc; i++)
    {
        can_frame.data[i] = 0;
    }
    if (rwflag == SDO_W_FUNC)
    {
        switch(len)
        {
            case 1:
                can_frame.data[0] = 0x2f;
                break;
            case 2:
                can_frame.data[0] = 0x2b;
                break;
            case 3:
                can_frame.data[0] = 0x27;
                break;
            case 4:
                can_frame.data[0] = 0x23;
                break;
            default:break;
        }
    }
    else
    {
        can_frame.data[0] = 0x40;
    }
    can_frame.data[1] = index;
    can_frame.data[2] = index >> 8;
    can_frame.data[3] = subIndex;
    for(int i=0; i<len; i++)
    {
        can_frame.data[4+i] = (data >> (8 * i)) & 0xff;
    }
    sdoSendQueue.push(can_frame);
    //driver_->send(can_frame); 
}

void Canopen::sdoPopSend()
{
    if(!sdoSendQueue.empty())
    {
        RCLCPP_INFO_STREAM(node_->get_logger(),"queue size is " << sdoSendQueue.size());
        can::Frame can_frame = sdoSendQueue.front();
        driver_->send(can_frame);
        sdoSendQueue.pop();
        RCLCPP_INFO_STREAM(node_->get_logger(),"queue size is " << sdoSendQueue.size());
    }
}

int Canopen::canopenRpdoToQueueIdx(uint8_t nodeId, uint8_t pdoSer)
{
    for(uint8_t i=0; i<COMM_PDO_QUEUE_SIZE; i++)
    {
        if((nodeId == m_commPort.ctl.RpdoQueueMap[i][0]) && (pdoSer == m_commPort.ctl.RpdoQueueMap[i][1]))
        {
            return i;
        }
    }
    return -1;
}

int Canopen::RpdoDataUpdate(uint8_t ser,uint8_t nodeId, uint8_t pdoSer, uint8_t startByte, uint8_t len, int32_t data)
{
    if(ser >= COMM_PDO_SUM)
        return -1;
    if((startByte + len) > 8)
        return -1;
    pCOMM_RPDOQUEUE pdoQueue = &m_commPort.ctl.rpdQueue;
    pdoQueue->buf[ser].nodeId = nodeId;
    pdoQueue->buf[ser].pdoSer = pdoSer;
    if((startByte+len) > pdoQueue->buf[ser].len)
    {
        pdoQueue->buf[ser].len = startByte + len;
    }
    if(len > 4)
    {
        return -1;
    }
    for(uint8_t i=0; i< len; i++)
    {
        pdoQueue->buf[ser].data[startByte + i] = ((data >> 8*i) & 0xFF);
    }
    pdoQueue->period = COMM_PDO_PERIOD_TRANSMISSION;
    return 0;
}

int Canopen::pdoSend(uint8_t nodeId, CANOPEN_RPDO_E index, int32_t data)
{
    if(index > E_RPDO_BUTT)
    {
        return -1;
    }
    int idx = nodeIdToIdx(nodeId);
    if(idx < 0)
    {
        return -1;
    }
    pCANOPEN_RPDOMAPCTL rPdoMapCtl = &m_commPort.ctl.rPdoCtl[idx].rPdoMapCtl[index];
    int pdoQueueIdx = canopenRpdoToQueueIdx(nodeId, rPdoMapCtl->pdoSer);
    if(pdoQueueIdx < 0)
    {
        return -1;
    }
    if(RpdoDataUpdate(pdoQueueIdx, nodeId, rPdoMapCtl->pdoSer, rPdoMapCtl->startByte, rPdoMapCtl->len, data)==0)
    {
        return 0;
    }
    return -1;
}

int Canopen::commTsdoAnalyse(COMM_SDODATA sdoData)
{
    int idx = nodeIdToIdx(sdoData.nodeId);
    if( idx < 0)
    {
        return -1;
    }
    pCOMM_TSDODATA_LIST tSdo = &m_commPort.tSdoList[idx];
    switch(sdoData.index)
    {
        case 0x6041:
        {
            if(sdoData.subIndex == 0x00)
            {
                tSdo->sdo604100 = (sdoData.data & 0xFFFF);
            }
        }break;
        case 0x6061:
        {
            if(sdoData.subIndex == 0x00)
            {
                tSdo->sdo606100 = (sdoData.data & 0xFF);
            }
        }break;
        default: break;
    }
    return 0;
}

int Canopen::commTpdoAnalyse(COMM_PDODATA pdoData)
{
    if(pdoData.pdoSer >= PER_NODE_PDO_NUM)
    {
        return -1;
    }
    int idx = nodeIdToIdx(pdoData.nodeId);
    if(idx < 0)
    {
        return -1;
    }
    uint8_t startByte = 0;
    uint8_t len = 0;
    pCOMM_PDOMAPCFG tPdoCfg = &m_commPort.cfg[idx].tPdoMapCfg[pdoData.pdoSer];
    pCOMM_TPDODATA_LIST tPdo = &m_commPort.tPdoList[idx]; 
    for(uint8_t i=0; i<PER_PDO_OBJ_NUM; i++)
    {
        if(tPdoCfg->index[i] == 0)
        {
            break;
        }
        startByte = tPdoCfg->startByte[i];
        len = tPdoCfg->len[i];
        if (tPdoCfg->index[i] == 0x6041)
        {
            uint16_t  pdo604100 = 0;
            for(uint8_t j = startByte; j< (startByte+len); j++)
            {
                pdo604100 |= pdoData.data[j]<<((j-startByte)*8);
            }
            tPdo->pdo604100 = pdo604100;
        }
        if (tPdoCfg->index[i] == 0x6063)
        {
            int  pdo606300 = 0;
            for(uint8_t j = startByte; j< (startByte+len); j++)
            {
                pdo606300 |= pdoData.data[j]<<((j-startByte)*8);
            }
            tPdo->pdo606300 = pdo606300;
        }
        if (tPdoCfg->index[i] == 0x603f)
        {
            uint16_t  pdo603f00 = 0;
            for(uint8_t j = startByte; j< (startByte+len); j++)
            {
                pdo603f00 |= pdoData.data[j]<<((j-startByte)*8);
            }
            tPdo->pdo603f00 = pdo603f00;
            if(tPdo->pdo603f00)
            {
                //RCLCPP_ERROR(node_->get_logger(), "node %d rcv err code %d",pdoData.nodeId, tPdo->pdo603f00);
            }
        }
        if (tPdoCfg->index[i] == 0x6081)
        {
            uint32_t  pdo608100 = 0;
            for(uint8_t j = startByte; j< (startByte+len); j++)
            {
                pdo608100 |= pdoData.data[j]<<((j-startByte)*8);
            }
            tPdo->pdo608100 = pdo608100;
        }
        if (tPdoCfg->index[i] == 0x60FD)
        {
            uint32_t  pdo60FD00 = 0;
            for(uint8_t j = startByte; j< (startByte+len); j++)
            {
                pdo60FD00 |= pdoData.data[j]<<((j-startByte)*8);
            }
            tPdo->pdo60FD00 = pdo60FD00;
        }
        if (tPdoCfg->index[i] == 0x6061)
        {
            int8_t  pdo606100 = 0;
            for(uint8_t j = startByte; j< (startByte+len); j++)
            {
                pdo606100 |= pdoData.data[j]<<((j-startByte)*8);
            }
            tPdo->pdo606100 = pdo606100;
        }
        if (tPdoCfg->index[i] == 0x606c)
        {
            int  pdo606c00 = 0;
            for(uint8_t j = startByte; j< (startByte+len); j++)
            {
                pdo606c00 |= pdoData.data[j]<<((j-startByte)*8);
            }
            tPdo->pdo606c00 = pdo606c00;
        }
        if (tPdoCfg->index[i] == 0x2101)
        {
            int16_t  pdo210100 = 0;
            for(uint8_t j = startByte; j< (startByte+len); j++)
            {
                pdo210100 |= pdoData.data[j]<<((j-startByte)*8);
            }
            tPdo->pdo210100 = pdo210100;
        }
        if (tPdoCfg->index[i] == 0x2102)
        {
            uint16_t  pdo210200 = 0;
            for(uint8_t j = startByte; j< (startByte+len); j++)
            {
                pdo210200 |= pdoData.data[j]<<((j-startByte)*8);
            }
            tPdo->pdo210200 = pdo210200;
        }
        if (tPdoCfg->index[i] == 0x3101)
        {
            if (tPdoCfg->subIndex[i] == 0x00)
            {
                int32_t  pdo310100 = 0;
                for(uint8_t j = startByte; j< (startByte+len); j++)
                {
                    pdo310100 |= pdoData.data[j]<<((j-startByte)*8);
                }
                tPdo->pdo310100 = pdo310100;
            }
        }
        if (tPdoCfg->index[i] == 0x3102)
        {
            if (tPdoCfg->subIndex[i] == 0x00)
            {
                int16_t  pdo310200 = 0;
                for(uint8_t j = startByte; j< (startByte+len); j++)
                {
                    pdo310200 |= pdoData.data[j]<<((j-startByte)*8);
                }
                tPdo->pdo310200 = pdo310200;
            }
        }
        if (tPdoCfg->index[i] == 0x3100)
        {
            if (tPdoCfg->subIndex[i] == 0x00)
            {
                int16_t  pdo310000 = 0;
                for(uint8_t j = startByte; j< (startByte+len); j++)
                {
                    pdo310000 |= pdoData.data[j]<<((j-startByte)*8);
                }
                tPdo->pdo310000 = pdo310000;
            }
        }
    }
    return 0;
}

int Canopen::commTnmtAnalyse(COMM_NMTDATA nmtData)
{
    int idx = nodeIdToIdx(nmtData.nodeId);
    if( idx < 0)
    {
        return -1;
    }
    m_commPort.tNmtList[idx].nmtTime = node_->now();
    int nodeState = nmtData.data & 0x7F;
    // RCLCPP_INFO(node_->get_logger(), "node %d state is %d",nmtData.nodeId, nodeState);
    return 0;
}

void Canopen::emcy_handle(const can::Frame& msg)
{
    uint8_t nodeId = msg.id - 0x80;
    uint16_t Emergency_code = msg.data[0] | msg.data[1]<<8;
    if(Emergency_code)
    {
        RCLCPP_ERROR(node_->get_logger(), 
            "EMCY received node: %d emcy code:%d", nodeId, Emergency_code); //to do update
    }
}

void Canopen::canmsg_cb(const can::Frame& msg)
{
    if (!msg.isValid())
    {
        RCLCPP_ERROR(node_->get_logger(), 
            "Invalid frame from SocketCAN: id: %#04x, length: %d, is_error: %d", 
            msg.id, msg.dlc, msg.is_error);
        return;
    }
    if((msg.id > 0x580) && (msg.id < 0x600))
    {
        COMM_SDODATA sdoData;
        sdoData.nodeId = msg.id - 0x580;
        sdoData.index = (msg.data[2]<<8) | msg.data[1];
        sdoData.subIndex = msg.data[3];
        sdoData.data = msg.data[7]<<24|msg.data[6]<<16|msg.data[5]<<8|msg.data[4];
        if(msg.data[0] == 0x80)
        {
            RCLCPP_WARN(node_->get_logger(), 
                "sdo response err, nodeId is %d index is %x subindex is %x",
                sdoData.nodeId, sdoData.index, sdoData.subIndex);
            return;
        }
        commTsdoAnalyse(sdoData);
    }
    else if((msg.id > 0x180) && (msg.id < 0x4FF))
    {
        COMM_PDODATA pdoData;
        if((msg.id> 0x180) && (msg.id < 0x1FF))
        {
            pdoData.nodeId = msg.id - 0x180;
            pdoData.pdoSer = 0;
        }
        else if((msg.id > 0x280) && (msg.id < 0x2FF))
        {
            pdoData.nodeId = msg.id - 0x280;
            pdoData.pdoSer = 1;            
        }
        else if((msg.id > 0x380) && (msg.id < 0x3FF))
        {
            pdoData.nodeId = msg.id - 0x380;
            pdoData.pdoSer = 2;            
        }
        else if((msg.id > 0x480) && (msg.id < 0x4FF))
        {
            pdoData.nodeId = msg.id - 0x480;
            pdoData.pdoSer = 3;            
        }
        else 
            return;
        for(uint8_t i=0; i<msg.dlc; i++)
        {
            pdoData.data[i] = msg.data[i];
        }
        pdoData.len = msg.dlc;
        commTpdoAnalyse(pdoData);
    }
    else if((msg.id > 0x700) && (msg.id < 0x7FF))
    {
        COMM_NMTDATA nmtData = {0};
        nmtData.nodeId = msg.id -0x700;
        nmtData.data = msg.data[0];
        //nmt data analyfunc
        commTnmtAnalyse(nmtData);
        return;
    }
    // EMCY
    else if((msg.id > 0x80) && (msg.id < 0xFF))
    {
        emcy_handle(msg);
    }
}

void Canopen::paramInit()
{
	std::vector<std::string> joint_names;
    node_->get_parameter("joint_names", joint_names);
    for(uint8_t i=0; i<joint_names.size(); i++)
    {
        int nodeId;
        node_->get_parameter_or(joint_names[i] + "/nodeId", nodeId, 0);
        m_commPort.cfg[i].nodeId = nodeId;
        RCLCPP_INFO_STREAM(node_->get_logger(),joint_names[i] <<" nodeId is " << nodeId);

        node_->get_parameter_or(joint_names[i] + "/nodeGuarding_mode", m_commPort.nodeGuardingCfg[i].nodeGuarding_mode, 0);
        RCLCPP_INFO_STREAM(node_->get_logger(),joint_names[i] <<" nodeGuarding_mode is "<< m_commPort.nodeGuardingCfg[i].nodeGuarding_mode);

        int nodeGuarding_time;
        node_->get_parameter_or(joint_names[i] + "/nodeGuarding_time", nodeGuarding_time, 0);
        m_commPort.nodeGuardingCfg[i].nodeGuarding_time = nodeGuarding_time;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_names[i] <<" nodeGuarding_time is "<< m_commPort.nodeGuardingCfg[i].nodeGuarding_time);

        int coefficient_of_time;
        node_->get_parameter_or(joint_names[i] + "/coefficient_of_time", coefficient_of_time, 0);
        m_commPort.nodeGuardingCfg[i].coefficient_of_time = coefficient_of_time;
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_names[i] <<" coefficient_of_time is "<< coefficient_of_time);

        node_->get_parameter_or(joint_names[i] + "/pdoToMap", m_commPort.cfg[i].sdoToPdoMap, false);
        RCLCPP_INFO_STREAM(node_->get_logger(), joint_names[i] <<" pdoToMap is "<< m_commPort.cfg[i].sdoToPdoMap);

        for(uint8_t pdoSer=0; pdoSer<PER_NODE_PDO_NUM; pdoSer++)
        {
            m_commPort.cfg[i].tPdoMapCfg[pdoSer].pdoSer = pdoSer;
            m_commPort.cfg[i].rPdoMapCfg[pdoSer].pdoSer = pdoSer;
            // XmlRpc::XmlRpcValue Tpdo;
            if (node_->has_parameter(joint_names[i] + "/TpdoCfg" + std::to_string(pdoSer)))
            {
                std::vector<int64_t> tpdo_values;
                node_->get_parameter(joint_names[i] + "/TpdoCfg" + std::to_string(pdoSer), tpdo_values);
                // assert(Tpdo.getType() == XmlRpc::XmlRpcValue::TypeArray);
                // for(uint8_t objSer=0; objSer<Tpdo.size(); objSer++)
                uint8_t objSer=0;
                for(auto value : tpdo_values)
                {         
                    m_commPort.cfg[i].tPdoMapCfg[pdoSer].index[objSer] = static_cast<uint16_t>((value >> 16) & 0xFFFF);
                    m_commPort.cfg[i].tPdoMapCfg[pdoSer].subIndex[objSer] = static_cast<uint16_t>((value >> 8) & 0xFF);
                    m_commPort.cfg[i].tPdoMapCfg[pdoSer].startByte[objSer] = static_cast<uint16_t>((value >> 4) & 0xF);
                    m_commPort.cfg[i].tPdoMapCfg[pdoSer].len[objSer] = static_cast<uint16_t>(value & 0xF);
                    RCLCPP_INFO(node_->get_logger(),
                        "tpdo%d map index is %x subIndex is %x startByte is %d len is %d",
                        pdoSer,m_commPort.cfg[i].tPdoMapCfg[pdoSer].index[objSer],
                        m_commPort.cfg[i].tPdoMapCfg[pdoSer].subIndex[objSer],
                        m_commPort.cfg[i].tPdoMapCfg[pdoSer].startByte[objSer],
                        m_commPort.cfg[i].tPdoMapCfg[pdoSer].len[objSer]); 
                    objSer++;
                }  
            }

            if (node_->has_parameter(joint_names[i] + "/RpdoCfg" + std::to_string(pdoSer)))
            {
                // XmlRpc::XmlRpcValue Rpdo;
                std::vector<int64_t> rpdo_values;
                node_->get_parameter(joint_names[i] + "/RpdoCfg" + std::to_string(pdoSer), rpdo_values);
                // assert(Rpdo.getType() == XmlRpc::XmlRpcValue::TypeArray);
                uint8_t objSer=0;
                for (auto value : rpdo_values) {
                    m_commPort.cfg[i].rPdoMapCfg[pdoSer].index[objSer] = static_cast<uint16_t>((value >> 16) & 0xFFFF);
                    m_commPort.cfg[i].rPdoMapCfg[pdoSer].subIndex[objSer] = static_cast<uint8_t>((value >> 8) & 0xFF);
                    m_commPort.cfg[i].rPdoMapCfg[pdoSer].startByte[objSer] = static_cast<uint8_t>((value >> 4) & 0xF);
                    m_commPort.cfg[i].rPdoMapCfg[pdoSer].len[objSer] = static_cast<uint8_t>(value & 0xF);
                    RCLCPP_INFO(node_->get_logger(), 
                        "rpdo%d map index is %x subIndex is %x startByte is %d len is %d",
                        pdoSer,m_commPort.cfg[i].rPdoMapCfg[pdoSer].index[objSer],
                        m_commPort.cfg[i].rPdoMapCfg[pdoSer].subIndex[objSer],
                        m_commPort.cfg[i].rPdoMapCfg[pdoSer].startByte[objSer],
                        m_commPort.cfg[i].rPdoMapCfg[pdoSer].len[objSer]);    
                    objSer++;   
                }
            }
        }
    }
    RCLCPP_INFO(node_->get_logger(), "init canopen params success.");
}

void Canopen::checkNodeStatus()
{
    for(uint8_t idx=0; idx<CANOPEN_NODES; idx++)
    {
        bool NodeLoss = false;
        pCOMM_NODEGUARDING_CFG cfg = &m_commPort.nodeGuardingCfg[idx];
        if(!cfg->nodeGuarding_mode || !(m_commPort.tNmtList[idx].nmtTime.seconds()))
        {
            continue;
        }
        if((NodeGuarding == cfg->nodeGuarding_mode) && ((node_->now() - m_commPort.tNmtList[idx].nmtTime).seconds() * 1000) > (cfg->nodeGuarding_time * cfg->coefficient_of_time))
        {
            NodeLoss = true;
        }
        else if((Heartbeat == cfg->nodeGuarding_mode) && ((node_->now() - m_commPort.tNmtList[idx].nmtTime).seconds() * 1000) > (2*SLAVE_HEARTBEAT_CYCLE))
        {
            NodeLoss = true;
        }
        if(NodeLoss)
        {
            m_commPort.tPdoList[idx].pdo603f00 = 0x8100;     
        }
    }
}

int Canopen::commPdoRunTime(void)
{
    pCOMM_RPDOQUEUE pdoQueue = &m_commPort.ctl.rpdQueue;
    pCOMM_PDOCTL ctl= &m_commPort.ctl;
    static uint16_t idx = 0;
    can::Frame can_frame;
    if(ctl->pdoStartFlag == false)
    {
        return -1;
    }
    for(uint8_t idx=0; idx<COMM_PDO_SUM; idx++)
    {
        uint8_t data[10] = {0};
        uint8_t len = pdoQueue->buf[idx].len;
        uint8_t nodeId = pdoQueue->buf[idx].nodeId;
        uint8_t pdoSer = pdoQueue->buf[idx].pdoSer;
        uint16_t cobId = 0x200 + 0x100 * pdoSer + nodeId;
        if((0 != len) && (0 != nodeId))
        {
            can_frame.id = cobId;
            can_frame.is_extended = false;
            can_frame.is_rtr = false;
            can_frame.is_error = false;
            can_frame.dlc = len;
            for(uint8_t i = 0; i< len; i++)
            {
                can_frame.data[i] = pdoQueue->buf[idx].data[i];
            }
            driver_->send(can_frame);
        }
    }
    return 0;
}

void Canopen::nodeProtectionRuntime()
{
    pCOMM_NODEPROTECTION ctl = &m_commPort.nodeProCtl;
    for(uint8_t i=0; i<CANOPEN_NODES; i++)
    {
        if(!ctl->nodeProtectionPeriod[i])
        {
            continue;
        }
        if(((node_->now() - ctl->nodePreTime[i]).seconds() * 1000) >= ctl->nodeProtectionPeriod[i])
        {
            ctl->nodePreTime[i] = node_->now();
            nodeGuarding_send(m_commPort.cfg[i].nodeId);
        }
    }
}

void Canopen::thread_runtime(void)
{
    // syncSend();
    commPdoRunTime();
    nodeProtectionRuntime();
    checkNodeStatus();
}

void Canopen::thread_fun(Canopen *pThis)
{
    if (!pThis)
    {
        return;
    }

    while (!pThis->m_quit_mutex.try_lock_for(std::chrono::milliseconds(20)))
    {
        pThis->thread_runtime();
    }
}

void Canopen::sdoSendFunc(Canopen *pThis)
{
    if (!pThis)
    {
        return;
    }

    while (!pThis->m_sdoSend_quit_mutex.try_lock_for(std::chrono::milliseconds(2)))
    {
        pThis->sdoPopSend();
        rclcpp::spin_some(pThis->GetNode()); 
    }
}

int Canopen::canopenRpdoQueueIdxMap(uint8_t nodeId, uint8_t pdoSer)
{
    int queueIdx = canopenRpdoToQueueIdx(nodeId, pdoSer);
    if(queueIdx >= 0)
    {
        return 0;
    }
    uint8_t rpdoQueueIdxMax = m_commPort.ctl.RpdoQueueIdxMax;
    if(rpdoQueueIdxMax > COMM_PDO_QUEUE_SIZE)
    {
        return -1;
    }
    m_commPort.ctl.RpdoQueueMap[rpdoQueueIdxMax][0] = nodeId;
    m_commPort.ctl.RpdoQueueMap[rpdoQueueIdxMax][1] = pdoSer;
    m_commPort.ctl.RpdoQueueIdxMax++;

    return 0;
}

int Canopen::canopenRpdoDataMap(uint8_t ser, uint8_t pdoSer, uint16_t index, uint8_t subIndex, uint8_t startByte, uint8_t len)
{
    pCOMM_PDO_CFG cfg = &m_commPort.cfg[ser];
    pCOMM_RPDOCTL ctl = &m_commPort.ctl.rPdoCtl[ser];
    uint8_t pdoUseFlag = 1;
    switch (index)
    {
    case 0:
        pdoUseFlag = 0; //没有pdo映射
        break;
    case 0x60ff:
        if(subIndex == 0x00)
        {
            ctl->rPdoMapCtl[E_RPDO_60FF00].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_60FF00].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_60FF00].len = len;
        }
        break;
    case 0x2001:
        if(subIndex == 0x00)
        {
            ctl->rPdoMapCtl[E_RPDO_200100].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_200100].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_200100].len = len;
        }
        break;
    case 0x2002:
        if(subIndex == 0x00)
        {
            ctl->rPdoMapCtl[E_RPDO_200200].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_200200].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_200200].len = len;
        }
        break;
    case 0x2003:
        if(subIndex == 0x00)
        {
            ctl->rPdoMapCtl[E_RPDO_200300].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_200300].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_200300].len = len;
        }
        break;
    case 0x607A:
        if(subIndex == 0x00)
        {
            ctl->rPdoMapCtl[E_RPDO_607A00].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_607A00].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_607A00].len = len;
        }
        break;
    case 0x6040:
        if(subIndex == 0x00)
        {
            ctl->rPdoMapCtl[E_RPDO_604000].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_604000].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_604000].len = len;
        }
        break;
    case 0x3000:
        if(subIndex == 0x00)
        {
            ctl->rPdoMapCtl[E_RPDO_300000].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_300000].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_300000].len = len;
        }
        else if(subIndex == 0x01)
        {
            ctl->rPdoMapCtl[E_RPDO_300001].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_300001].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_300001].len = len;
        }
        else if(subIndex == 0x02)
        {
            ctl->rPdoMapCtl[E_RPDO_300002].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_300002].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_300002].len = len;
        }
        else if(subIndex == 0x03)
        {
            ctl->rPdoMapCtl[E_RPDO_300003].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_300003].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_300003].len = len;
        }
        else if(subIndex == 0x04)
        {
            ctl->rPdoMapCtl[E_RPDO_300004].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_300004].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_300004].len = len;
        }
        else if(subIndex == 0x05)
        {
            ctl->rPdoMapCtl[E_RPDO_300005].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_300005].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_300005].len = len;
        }
        break;
    case 0x3001:
        if(subIndex == 0x00)
        {
            ctl->rPdoMapCtl[E_RPDO_300100].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_300100].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_300100].len = len;
        }
        else if(subIndex == 0x01)
        {
            ctl->rPdoMapCtl[E_RPDO_300101].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_300101].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_300101].len = len;
        }
        else if(subIndex == 0x02)
        {
            ctl->rPdoMapCtl[E_RPDO_300102].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_300102].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_300102].len = len;
        }
        else if(subIndex == 0x03)
        {
            ctl->rPdoMapCtl[E_RPDO_300103].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_300103].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_300103].len = len;
        }
        else if(subIndex == 0x04)
        {
            ctl->rPdoMapCtl[E_RPDO_300104].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_300104].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_300104].len = len;
        }
        else if(subIndex == 0x05)
        {
            ctl->rPdoMapCtl[E_RPDO_300105].pdoSer = pdoSer;
            ctl->rPdoMapCtl[E_RPDO_300105].startByte = startByte;
            ctl->rPdoMapCtl[E_RPDO_300105].len = len;
        }
        break;
    default:
        pdoUseFlag = 0;
        break;
    }
    if(pdoUseFlag)
    {
        canopenRpdoQueueIdxMap(cfg->nodeId, pdoSer);
    }
    return 0;
}


int Canopen::canopenRpdoDataMapInit(uint8_t nodeId)
{
    int idx = nodeIdToIdx(nodeId);
    if( idx < 0)
    {
        return -1;
    }
    for(uint8_t j = 0; j < PER_NODE_PDO_NUM; j++)
    {
        pCOMM_PDOMAPCFG rPdoCfg = &m_commPort.cfg[idx].rPdoMapCfg[j];
        if(rPdoCfg->index[0] == 0)
            continue;
        for(uint8_t k=0; k<PER_PDO_OBJ_NUM; k++)
        {
            canopenRpdoDataMap(idx, rPdoCfg->pdoSer, rPdoCfg->index[k],rPdoCfg->subIndex[k],rPdoCfg->startByte[k],rPdoCfg->len[k]);
        }
    }
    return 0;
}

int Canopen::canopenPdoMapSend(uint8_t nodeId, uint8_t trPdoFlag, COMM_PDOMAPCFG pdoCfg)
{
    int idx = nodeIdToIdx(nodeId);
    if(idx<0)
    {
        return -1;
    }
    if(pdoCfg.index[0] != 0)
    {
        uint8_t len = 0;
        int32_t data = 0;
        uint8_t idxNum = 0;
        if(trPdoFlag == 0) //rpdo,发送给从站如发给伺服的pdo
        {
            sdoSend(nodeId, SDO_W_FUNC, 1, (0x1400 + pdoCfg.pdoSer), 0x02, 0x01);
            sdoSend(nodeId, SDO_W_FUNC, 1, (0x1600 + pdoCfg.pdoSer), 0x00, 0x00);
            for(uint8_t i=0; i<PER_PDO_OBJ_NUM; i++)
            {
                if(pdoCfg.index[i] != 0)
                {
                    len = pdoCfg.len[i];
                    if(len == 1)
                        len = 0x08;
                    else if(len == 2)
                        len = 0x10;
                    else
                        len = 0x20;
                    data =((pdoCfg.index[i] << 16)|(pdoCfg.subIndex[i] << 8)|len);
                    sdoSend(nodeId, SDO_W_FUNC, 4, (0x1600+pdoCfg.pdoSer), (i+1), data);
                    idxNum++;
                }
            }
            sdoSend(nodeId, SDO_W_FUNC, 1, (0x1600+pdoCfg.pdoSer), 0x00, idxNum);
            sdoSend(nodeId, SDO_W_FUNC, 4, (0x1400 + pdoCfg.pdoSer), 0x01, 0x00000200+0x100*pdoCfg.pdoSer+nodeId);
        }
        else //tpdo.从站发出的pdo，如伺服发出的数据
        {
            sdoSend(nodeId, SDO_W_FUNC, 4, (0x1800+pdoCfg.pdoSer), 0x01, 0x80000180+0x100*pdoCfg.pdoSer+nodeId);
            sdoSend(nodeId, SDO_W_FUNC, 1, (0x1A00+pdoCfg.pdoSer), 0x00, 0x00);
            for(uint8_t i=0; i<PER_PDO_OBJ_NUM; i++)
            {
                if(pdoCfg.index[i] != 0)
                {
                    len = pdoCfg.len[i];
                    if(len == 1)
                        len = 0x08;
                    else if(len == 2)
                        len = 0x10;
                    else
                        len = 0x20;
                    data =((pdoCfg.index[i] << 16)|(pdoCfg.subIndex[i] << 8)|len);
                    sdoSend(nodeId, SDO_W_FUNC, 4, (0x1A00+pdoCfg.pdoSer), (i+1), data);
                    idxNum++;
                }
            }
            sdoSend(nodeId, SDO_W_FUNC, 1, (0x1A00+pdoCfg.pdoSer), 0x00, idxNum);
            sdoSend(nodeId, SDO_W_FUNC, 1, (0x1800+pdoCfg.pdoSer), 0x02, 0x01);
            sdoSend(nodeId, SDO_W_FUNC, 4, (0x1800+pdoCfg.pdoSer), 0x01, 0x00000180+0x100*pdoCfg.pdoSer+nodeId); //配置tpdo的cobid
        }
    }
    else
    {
        if(trPdoFlag == 1)
        {
            sdoSend(nodeId, SDO_W_FUNC, 4, (0x1800+pdoCfg.pdoSer), 0x01, 0x80000180+0x100*pdoCfg.pdoSer+nodeId);
        }
    }
    return 0;
}

int Canopen::canopenPdoMap(uint8_t nodeId)
{

    int idx = nodeIdToIdx(nodeId);
    if(idx < 0)
    {
        return -1;
    }
    pCOMM_PDO_CFG cfg = &m_commPort.cfg[idx];
    for(uint8_t i=0; i<PER_NODE_PDO_NUM; i++)
    {
        COMM_PDOMAPCFG rPdoCfg = cfg->rPdoMapCfg[i];
        COMM_PDOMAPCFG tPdoCfg = cfg->tPdoMapCfg[i]; 
        canopenPdoMapSend(cfg->nodeId, 0, rPdoCfg);
        canopenPdoMapSend(cfg->nodeId, 1, tPdoCfg);               
    }
    return 0;
}

void Canopen::nodeGuarding_start(uint8_t nodeId, uint16_t period)
{
    int idx = nodeIdToIdx(nodeId);
    if(idx < 0)
    {
        return;
    }
    pCOMM_NODEPROTECTION ctl = &m_commPort.nodeProCtl;
    ctl->nodePreTime[idx] = node_->now();
    ctl->nodeProtectionPeriod[idx] = period;
}

int Canopen::canopenNodeGuarding(uint8_t nodeId)
{
    int idx = nodeIdToIdx(nodeId);
    if(idx < 0)
    {
        return -1;
    }
    pCOMM_NODEGUARDING_CFG cfg = &m_commPort.nodeGuardingCfg[idx];
    //开启节点保护
    if(NodeGuarding == cfg->nodeGuarding_mode)
    {
        if((cfg->nodeGuarding_time) && (cfg->coefficient_of_time))
        {
            sdoSend(nodeId, SDO_W_FUNC, 2, 0x100c, 0x00, cfg->nodeGuarding_time); //节点保护时间
            sdoSend(nodeId, SDO_W_FUNC, 1, 0x100d, 0x00, cfg->coefficient_of_time); //节点保护时间系数
            sdoSend(nodeId, SDO_W_FUNC, 2, 0x6007, 0x00, 1);
            nodeGuarding_start(nodeId, cfg->nodeGuarding_time);
            return 0;    
        }
        RCLCPP_WARN(node_->get_logger(), "Node protection parameters are incorrectly set");
    }
    //开启心跳保护
    else if(Heartbeat == cfg->nodeGuarding_mode)
    {
        sdoSend(nodeId, SDO_W_FUNC, 2, 0x1017, 0x00, SLAVE_HEARTBEAT_CYCLE); //开启节点心跳上报 
    }
    else
    {
        RCLCPP_WARN(node_->get_logger(), "The node protection mode is not configured");
    }
    return 0;
}

int Canopen::canopen_init(uint8_t nodeId)
{
    if(!driver_init())
    {
        return -1;
    }
    nmtSend(nodeId, NMT_RESET_APP);
    rclcpp::Duration duration(1, 0); // Correct constructor
    std::this_thread::sleep_for(duration.to_chrono<std::chrono::nanoseconds>());

    int idx = nodeIdToIdx(nodeId);
    pCOMM_PDO_CFG cfg = &m_commPort.cfg[idx];

    if(cfg->sdoToPdoMap)
    {
        nmtSend(nodeId, NMT_PRE_OPERATION);
        std::this_thread::sleep_for(duration.to_chrono<std::chrono::nanoseconds>()); 
        canopenPdoMap(nodeId);
    }
    canopenRpdoDataMapInit(nodeId); //发送给伺服的pdo，数据关联
    comm_PdoStart(true); //启动coComm层pdo发送
    canopenNodeGuarding(nodeId);
    std::this_thread::sleep_for(duration.to_chrono<std::chrono::nanoseconds>()); 
    nmtSend(nodeId, NMT_OPERATION); 
    return 0;
}

bool Canopen::driver_init(void)
{
    static bool initFlag = false;
    if(!initFlag)
    {
        std::string can_device;
        node_->get_parameter_or<std::string>("can_device", can_device, "can0");
        driver_ = std::make_shared<can::ThreadedSocketCANInterface> ();
        if (!driver_->init(can_device, 0, can::NoSettings::create()))  // initialize device at can_device, 0 for no loopback.
        {
            RCLCPP_ERROR(node_->get_logger(), "Failed to initialize can_device at %s", can_device.c_str());
            return false;
        }
        else
        {
            RCLCPP_INFO(node_->get_logger(), "Successfully connected to %s.", can_device.c_str());
        }
        frame_listener_ = driver_->createMsgListenerM(this, &Canopen::canmsg_cb);
        
        paramInit();
        synctimer = node_->create_wall_timer(
            std::chrono::milliseconds(20), 
            std::bind(&Canopen::syncSend, this));

        m_quit_mutex.lock();
        m_thread = std::thread(thread_fun, this);

        m_sdoSend_quit_mutex.lock();
        sdoSendThread = std::thread(sdoSendFunc, this);    
        initFlag = true;
    }

    return true;
}
